import CollisionMatrix from "../../collision-detector/collision-matrix";
import Contact from "./contact";
import CONSTANT from "../../constant";

export default class ContactManager {
    constructor(broadPhase) {
        this._broadPhase = broadPhase;
        this._collisionMatrix = new CollisionMatrix();
        this._numContacts = 0;
    }

    get numContacts() {
        return this._numContacts;
    }
    get contactList() {
        return this._contactList;
    }
    createContacts() {
        let pp = this._broadPhase._proxyPairList;
        while(pp != null) {
            let n = pp._next;
            let s1, s2;
            if (pp._p1._id < pp._p2._id) {
                s1 = pp._p1.userData;
                s2 = pp._p2.userData;
            } else {
                s1 = pp._p2.userData;
                s2 = pp._p1.userData;
            }
            if (this.shouldCollide(s1, s2)) {
                let b1 = s1._rigidBody, b2 = s2._rigidBody;
                let l;
                if (b1._numContactLinks < b2._numContactLinks) {
                    l = b1._contactLinkList;
                } else {
                    l = b2._contactLinkList;
                }
                let id1 = s1._id, id2 = s2._id;
                let found = false;
                while (l != null) {
                    let c = l._contact;
                    if (c._s1._id === id1 && c._s2._id === id2) {
                        c._latest = true;
                        found = true;
                        break;
                    }
                    l = l._next;
                }
                if (!found) {
                    let first = this._contactPool;
                    if (first != null) {
                        this._contactPool = first._next;
                        first._next = null;
                    } else {
                        first = new Contact();
                    }
                    let c = first;
                    if (this._contactList == null) {
                        this._contactList = this._contactListLast = c;
                    } else {
                        this._contactListLast._next = c;
                        c._prev = this._contactListLast;
                        this._contactListLast = c;
                    }
                    c._latest = true;
                    let detector = this._collisionMatrix.getDetector(s1._geom._type, s2._geom._type);
                    c._s1 = s1;
                    c._s2 = s2;
                    c._b1 = s1._rigidBody;
                    c._b2 = s2._rigidBody;
                    c._touching = false;
                    if (c._b1._contactLinkList == null) {
                        c._b1._contactLinkList = c._b1._contactLinkListLast = c._link1;
                    } else {
                        c._b1._contactLinkListLast._next = c._link1;
                        c._link1._prev = c._b1._contactLinkListLast;
                        c._b1._contactLinkListLast = c._link1;
                    }
                    if (c._b2._contactLinkList == null) {
                        c._b2._contactLinkList = c._b2._contactLinkListLast = c._link2;
                    } else {
                        c._b2._contactLinkListLast._next = c._link2;
                        c._link2._prev = c._b2._contactLinkListLast;
                        c._b2._contactLinkListLast = c._link2;
                    }
                    c._b1._numContactLinks++;
                    c._b2._numContactLinks++;
                    c._link1._other = c._b2;
                    c._link2._other = c._b1;
                    c._link1._contact = c._link2._contact = c;
                    c._detector = detector;
                    let _this = c._contactConstraint;
                    _this._s1 = s1;
                    _this._s2 = s2;
                    _this._b1 = _this._s1._rigidBody;
                    _this._b2 = _this._s2._rigidBody;
                    _this._tf1 = _this._b1._transform;
                    _this._tf2 = _this._b2._transform;
                    this._numContacts++;
                }
            }
            pp = n;
        }
    }
    destroyOutdatedContacts() {
        const RBS=CONSTANT.RIGID_BODY_TYPE_STATIC;
        let incremental = this._broadPhase._incremental;
        let c = this._contactList;
        while(c != null) {
            let n = c._next;
            if (c._latest) {
                c._latest = c._shouldBeSkipped = false;
            } else {
                if (!incremental) {
                    let prev = c._prev, next = c._next;
                    if (prev != null) prev._next = next;
                    if (next != null) next._prev = prev;
                    if (c === this._contactList) this._contactList = this._contactList._next;
                    if (c === this._contactListLast) this._contactListLast = this._contactListLast._prev;
                    c._next = c._prev = null;
                    if (c._touching) {
                        let cc1 = c._s1._contactCallback, cc2 = c._s2._contactCallback;
                        if (cc1 === cc2) cc2 = null;
                        if (cc1 != null) cc1.endContact(c);
                        if (cc2 != null) cc2.endContact(c);
                    }
                    let prev1 = c._link1._prev, next1 = c._link1._next;
                    if (prev1 != null) prev1._next = next1;
                    if (next1 != null) next1._prev = prev1;
                    if (c._link1 === c._b1._contactLinkList) c._b1._contactLinkList = c._b1._contactLinkList._next;
                    if (c._link1 === c._b1._contactLinkListLast) c._b1._contactLinkListLast = c._b1._contactLinkListLast._prev;
                    c._link1._next = c._link1._prev = null;
                    let prev2 = c._link2._prev, next2 = c._link2._next;
                    if (prev2 != null) prev2._next = next2;
                    if (next2 != null) next2._prev = prev2;
                    if (c._link2 === c._b2._contactLinkList) c._b2._contactLinkList = c._b2._contactLinkList._next;
                    if (c._link2 === c._b2._contactLinkListLast) c._b2._contactLinkListLast = c._b2._contactLinkListLast._prev;
                    c._link2._next = c._link2._prev = null;
                    c._b1._numContactLinks--;
                    c._b2._numContactLinks--;
                    c._link1._other = c._link2._other = null;
                    c._link1._contact = c._link2._contact = null;
                    c._s1 = c._s2 = null;
                    c._b1 = c._b2 = null;
                    c._touching = false;
                    c._cachedDetectorData._clear();
                    c._manifold._clear();
                    c._detector = null;
                    let _this = c._contactConstraint;
                    _this._s1 = _this._s2 = null;
                    _this._b1 = _this._b2 = null;
                    _this._tf1 = _this._tf2 = null;
                    c._next = this._contactPool;
                    this._contactPool = c;
                    this._numContacts--;
                } else {
                    let s1 = c._s1, s2 = c._s2;
                    let r1 = s1._rigidBody, r2 = s2._rigidBody;
                    if (!(!r1._sleeping && r1._type !== RBS) && !(!r2._sleeping && r2._type !== RBS)) {
                        c._shouldBeSkipped = true;
                    } else {
                        let ab1 = s1._aabb.elements, ab2 = s2._aabb.elements;
                        let px1 = s1._proxy.size, px2 = s2._proxy.size;
                        if (!(px1[0] < px2[3] && px1[3] > px2[0] && px1[1] < px2[4] && px1[4] > px2[1] && px1[2] < px2[5] && px1[5] > px2[2]) ||
                            !this.shouldCollide(s1, s2)) {
                            let prev = c._prev, next = c._next;
                            if (prev != null) prev._next = next;
                            if (next != null) next._prev = prev;
                            if (c === this._contactList) this._contactList = this._contactList._next;
                            if (c === this._contactListLast) this._contactListLast = this._contactListLast._prev;
                            c._next = c._prev = null;
                            if (c._touching) {
                                let cc1 = c._s1._contactCallback, cc2 = c._s2._contactCallback;
                                if (cc1 === cc2) cc2 = null;
                                if (cc1 != null) cc1.endContact(c);
                                if (cc2 != null) cc2.endContact(c);
                            }
                            let prev1 = c._link1._prev, next1 = c._link1._next;
                            if (prev1 != null) prev1._next = next1;
                            if (next1 != null) next1._prev = prev1;
                            if (c._link1 === c._b1._contactLinkList) c._b1._contactLinkList = c._b1._contactLinkList._next;
                            if (c._link1 === c._b1._contactLinkListLast) c._b1._contactLinkListLast = c._b1._contactLinkListLast._prev;
                            c._link1._next = c._link1._prev = null;
                            let prev2 = c._link2._prev, next2 = c._link2._next;
                            if (prev2 != null) prev2._next = next2;
                            if (next2 != null) next2._prev = prev2;
                            if (c._link2 === c._b2._contactLinkList) c._b2._contactLinkList = c._b2._contactLinkList._next;
                            if (c._link2 === c._b2._contactLinkListLast) c._b2._contactLinkListLast = c._b2._contactLinkListLast._prev;
                            c._link2._next = c._link2._prev = null;
                            c._b1._numContactLinks--;
                            c._b2._numContactLinks--;
                            c._link1._other = c._link2._other = null;
                            c._link1._contact = c._link2._contact = null;
                            c._s1 = c._s2 = null;
                            c._b1 = c._b2 = null;
                            c._touching = false;
                            c._cachedDetectorData._clear();
                            c._manifold._clear();
                            c._detector = null;
                            let _this = c._contactConstraint;
                            _this._s1 = _this._s2 = null;
                            _this._b1 = _this._b2 = null;
                            _this._tf1 = _this._tf2 = null;
                            c._next = this._contactPool;
                            this._contactPool = c;
                            this._numContacts--;
                        } else {
                            c._shouldBeSkipped = !(ab1[0] < ab2[3] && ab1[3] > ab2[0] && ab1[1] < ab2[4] && ab1[4] > ab2[1] && ab1[2] < ab2[5] && ab1[5] > ab2[2]);
                        }
                    }
                }
            }
            c = n;
        }
    }
    shouldCollide(s1,s2) {
        const RBD=CONSTANT.RIGID_BODY_TYPE_DYNAMIC;
        let r1 = s1._rigidBody, r2 = s2._rigidBody;
        if(r1 === r2) {
            return false;
        }
        if(r1._type !== RBD && r2._type !== RBD) {
            return false;
        }
        if((s1._collisionGroup & s2._collisionMask) === 0 || (s2._collisionGroup & s1._collisionMask) === 0) {
            return false;
        }
        let jl, other;
        if(r1._numJointLinks < r2._numJointLinks) {
            jl = r1._jointLinkList;
            other = r2;
        } else {
            jl = r2._jointLinkList;
            other = r1;
        }
        while(jl != null) {
            if(jl._other === other && !jl._joint._allowCollision) {
                return false;
            }
            jl = jl._next;
        }
        return true;
    }

    _updateContacts() {
        this._broadPhase.collectPairs();
        this.createContacts();
        this.destroyOutdatedContacts();
    }
    _postSolve() {
        let c = this._contactList;
        while(c != null) {
            let n = c._next;
            if(c._touching) {
                c._postSolve();
            }
            c = n;
        }
    }
}